#include "context.h"
#include "compiler.h"

#define CPU_REG_DEFUALT_VAL  0x05201314UL
#define CONTEXT_CPSR_DEFAULT 0x0000015fUL

context_t * g_cur_ctx, * g_next_ctx;

void
context_return(void)
{

}
void printk(char * fmt, ...);
static void
cpu_context_init(context_t * ctx,
            context_entry_t entry,
            void * stk_base, 
            unsigned int stk_size,
            void * arg)
{
    if ( !stk_base ) return;

    printk("stack base = 0x%p\r\n", stk_base);

    /* stack aligned up 8 byte*/
    ctx->stk_base = (void *)
        (((unsigned)stk_base + 0x7U) & (~0x7U));

    void * stk_end = (void *)
        ((char *)stk_base + stk_size);
    stk_end = (void *)
        (((unsigned)stk_end) & (~0x7U));

    if ( stk_end <= ctx->stk_base )
        return;

    if (( ctx->stk_size = stk_end - ctx->stk_base ) 
                < CONTEXT_STACK_SIZE_MIN)
        return;

    ctx->sp      = stk_end;
    ctx->cpu_ctx = stk_end;
    ctx->cpu_ctx -= 1;

    /* init cpu registers */
    ctx->cpu_ctx->r0  = (unsigned int)arg;
    ctx->cpu_ctx->r1  = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r2  = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r3  = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r4  = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r5  = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r6  = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r7  = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r8  = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r9  = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r10 = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r11 = CPU_REG_DEFUALT_VAL;
    ctx->cpu_ctx->r12 = CPU_REG_DEFUALT_VAL;

    ctx->cpu_ctx->lr  = (unsigned int)context_return;
    ctx->cpu_ctx->pc  = (unsigned int)entry;

    ctx->cpu_ctx->cpsr= CONTEXT_CPSR_DEFAULT;
    if ((unsigned int)entry & 1) {
        ctx->cpu_ctx->cpsr |= 0x20;
    }
    /* reload sp */
    ctx->sp = (unsigned int *)ctx->cpu_ctx;

    printk("context info:cpu context = 0x%p, sp = 0x%p, stack base = 0x%p, stack size = %d, r0 = 0x%p, r1 = 0x%p, entry = 0x%p\r\n", ctx->cpu_ctx, ctx->sp, ctx->stk_base, ctx->stk_size, 
    * (ctx->sp), * (ctx->sp + 1), * (ctx->sp + 14));
}

/* define context stack */
char stack1[1024];
char stack2[1024];

/* define context */
context_t ctx1, ctx2;
void delay1(int a)
{
    while(a--);
}
#include "gpio.h"
void gpio_pin_toggle(gpio_t * gpio, unsigned int pin);
int 
context1_entry(void * arg)
{
    while(1)
    {
        gpio_pin_toggle(GPIO5, 3);
        delay1(2000 * 0x7ff);
    }
    return 0;
}

int 
context2_entry(void * arg)
{
    while(1)
    {
        printk("hello context\r\n");
        delay1(500 * 0x7ff);
    }
    return 0;
}

void
sys_context_init(void)
{
    cpu_context_init(&ctx1, context1_entry, (void *)stack1, 1024, (void *)0);
    cpu_context_init(&ctx2, context2_entry, (void *)stack2, 1024, (void *)0);
    g_cur_ctx = &ctx2;
}

void
context_update_impl(void)
{
    if(g_cur_ctx == &ctx1)
        g_cur_ctx = &ctx2;
    else if(g_cur_ctx == &ctx2)
        g_cur_ctx = &ctx1;
}
extern void arch_start_first_context(void);
void
rqos_start(void)
{
    __asm volatile ("svc 0");
    // arch_start_first_context();
}

void print_current_context(context_t * ctx, unsigned int s)
{
    if(ctx == &ctx1)
        printk("now context is ctx1");
    else if(ctx == &ctx2)
        printk("now context is ctx1");
    else
        printk("r0 = 0x%p, r1 = 0x%p\r\n", (unsigned int *)ctx, s);
}